#include "PandaDrive.h"
#include "Joystick.h"
#include "Utility.h"
#include "WPIStatus.h"
#include "WPILib.h"
#include "Timer.h"
	const float insideSpeed = -0.1;
	const float Right_UpperRight = insideSpeed;
	const float Right_LowerRight = -insideSpeed;
	const float Right_UpperLeft = 1;
	const float Right_LowerLeft = -1;
	const float Right_Right = -1;
	const float Right_Left = 1;
	const float Right_Up = 1;
	const float Right_Down = -1;
	const float Right_Center = 0;
	const float Left_UpperRight = 1;
	const float Left_LowerRight = -1;
	const float Left_UpperLeft = insideSpeed;
	const float Left_LowerLeft = -insideSpeed;
	const float Left_Right = 1;
	const float Left_Left = -1;
	const float Left_Up = 1;
	const float Left_Down = -1;
	const float Left_Center = 0;

float PandaDrive::ArcadeDrive(float UpperRight, float LowerRight, float UpperLeft, float LowerLeft, float xValue, float yValue)
{
	float y_Left = yValue * UpperLeft + (1-yValue) * LowerLeft;
	float y_Right = yValue * UpperRight + (1-yValue) * LowerRight;
	float motorValue = xValue * y_Right + (1-xValue) * y_Left;
	return motorValue;
}

/*
 * Driving functions
 * These functions provide an interface to multiple motors that is used for C programming
 * The Drive(speed, direction) function is the main part of the set that makes it easy
 * to set speeds and direction independently in one call.
 */

/** Constructor for PandaDrive with 2 motors specified with channel numbers. 
 * Set up parameters for a two wheel drive system where the
 * left and right motor pwm channels are specified in the call.
 * This call assumes Victors for controlling the motors.
 * @param leftMotorChannel The PWM channel number on the default digital module that drives the left motor.
 * @param rightMotorChannel The PWM channel number on the default digital module that drives the right motor.
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value).
 */
PandaDrive::PandaDrive(UINT32 leftMotorChannel, UINT32 rightMotorChannel,
		GenericHID *leftStick, GenericHID *rightStick, 
		int tankButton, int arcadeButton, int nonSlipButton, int slipButton, int increaseAccelButton, int decreaseAccelButton,
		float sensitivity) 
                    : RobotDrive(leftMotorChannel, rightMotorChannel, sensitivity)
{
			m_leftStick = leftStick;
			m_rightStick = rightStick;
			m_lastLeftValue = 0;
			m_lastRightValue = 0;
			m_lastSpeed = 0;
			m_arcadeButton = arcadeButton;
			m_tankButton = tankButton;
			m_nonSlipButton = nonSlipButton;
			m_slipButton = slipButton;
			m_decreaseAccelButton = decreaseAccelButton;
			m_increaseAccelButton = increaseAccelButton;
			m_setLeftValue = 0;
			m_setRightValue = 0;
			m_nonSlip = false;
			m_tank = false;
			m_speed = 0;
			m_delay = 0;
			m_lastSpeed = 0;
			m_speed = 0;
			m_lastTime = GetTime();
			m_lastSpeed = 0;
			m_maxAccel = 2.0;
}

/**
 * Constructor for PandaDrive with 4 motors specified with channel numbers.
 * Set up parameters for a four wheel drive system where all four motor
 * pwm channels are specified in the call.
 * This call assumes Victors for controlling the motors.
 * @param frontLeftMotor Front left motor channel number on the default digital module
 * @param rearLeftMotor Rear Left motor channel number on the default digital module
 * @param frontRightMotor Front right motor channel number on the default digital module
 * @param rearRightMotor Rear Right motor channel number on the default digital module
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
 */

/**
 * Provide tank steering using the stored robot configuration.
 * Drive the robot using two joystick inputs. The Y-axis will be selected from
 * each Joystick object.
 * @param leftStick The joystick to control the left side of the robot.
 * @param rightStick The joystick to control the right side fo the robot.
 */
void PandaDrive::NonSlipTankDrive()
{
	if (m_leftStick == NULL || m_rightStick == NULL)
	{
		wpi_fatal(NullParameter);
		return;
	}

	float leftValue = m_leftStick->GetY();
	float rightValue = m_rightStick->GetY(); 

	// square the inputs (while preserving the sign) to increase fine control while permitting full power
	leftValue = Limit(leftValue);
	rightValue = Limit(rightValue);
	if (leftValue >= 0.0)
	{
		leftValue = (leftValue * leftValue);
	}
	else
	{
		leftValue = -(leftValue * leftValue);
	}

	if (rightValue >= 0.0)
	{
		rightValue = (rightValue * rightValue);
	}
	else
	{
		rightValue = -(rightValue * rightValue);
	}
	double m_time = GetTime();
	double m_delay = m_time - m_lastTime;
	if (m_nonSlip)
	{
		m_setRightValue = NonSlip(rightValue, m_delay, m_rightSpeed);
		m_setLeftValue = NonSlip(leftValue, m_delay, m_leftSpeed);
	}
	else 
	{
		m_setRightValue = rightValue;
		m_setLeftValue = leftValue;
	}
	m_lastTime = GetTime();
	m_rightSpeed = m_setRightValue;
	m_leftSpeed = m_setLeftValue;
	SetLeftRightMotorSpeeds(m_setRightValue, m_setLeftValue);
}

/**
 * Arcade drive implements single stick driving.
 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
 * for the rotate value.
 * (Should add more information here regarding the way that arcade drive works.)
 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
 * for forwards/backwards and the X-axis will be selected for rotation rate.  Pulling the trigger
 * enables "spin" functionality.
 */
void PandaDrive::NonSlipArcadeDrive()
{
	static int counter = 0;
	counter++;
	if (m_rightStick == NULL)
	{
		wpi_fatal(NullParameter);
		return;
	}
	
	float Y_Value = -m_rightStick->GetY();
	float X_Value = -m_rightStick->GetX();
	Y_Value = Limit(Y_Value);
	X_Value = Limit(X_Value);
	if (X_Value >= 0)
	{
		X_Value = (X_Value * X_Value);
	}
	else
	{
		X_Value = -(X_Value * X_Value);
	}
	
	if (Y_Value >= 0)
	{
		Y_Value = (Y_Value * Y_Value);
	}
	else 
	{
		Y_Value = -(Y_Value * Y_Value);
	}
	X_Value = -X_Value;
	if (X_Value >= 0.0)
	{
		if (Y_Value >= 0.0)
		{
			m_setRightValue = ArcadeDrive(Right_UpperRight,Right_Right,Right_Up,Right_Center,X_Value,Y_Value);
			m_setLeftValue = ArcadeDrive(Left_UpperRight,Left_Right,Left_Up,Left_Center,X_Value, Y_Value);
		} else if (Y_Value < 0.0)
		{
			m_setRightValue = ArcadeDrive(Right_LowerRight,Right_Right,Right_Down,Right_Center,X_Value, -Y_Value);
			m_setLeftValue = ArcadeDrive(Left_LowerRight,Left_Right,Left_Down,Left_Center,X_Value, -Y_Value);
		}
	} else if (X_Value < 0.0)
	{
		if (Y_Value >= 0.0)
		{
			m_setRightValue = ArcadeDrive(Right_UpperLeft,Right_Left,Right_Up,Right_Center,-X_Value, Y_Value);
			m_setLeftValue = ArcadeDrive(Left_UpperLeft,Left_Left,Left_Up,Left_Center,-X_Value, Y_Value);
		} else if (Y_Value < 0.0)
		{
			m_setRightValue = ArcadeDrive(Right_LowerLeft,Right_Left,Right_Down,Right_Center,-X_Value, -Y_Value);
			m_setLeftValue = ArcadeDrive(Left_LowerLeft,Left_Left,Left_Down,Left_Center,-X_Value, -Y_Value);
		}
	} 
	double m_time = GetTime();
	double m_delay = m_time - m_lastTime;
	if (m_nonSlip)
	{
	m_setRightValue = NonSlip(m_setRightValue, m_delay, m_rightSpeed);
	m_setLeftValue = NonSlip(m_setLeftValue, m_delay, m_leftSpeed);
	}
	m_lastTime = GetTime();
	if (counter % 5000 == 0)
	{
		printf("X , Y ; L , R : %+0.2f , %+0.2f ; %+0.2f , %+0.2f\n",X_Value, Y_Value ,m_setLeftValue, m_setRightValue);
	}
	
	SetLeftRightMotorSpeeds(m_setLeftValue, m_setRightValue);
}

float PandaDrive::NonSlip(float speed, double delay, float& lastSpeed)
{
	float setSpeed;
	float delAccel = m_maxAccel * (float)delay;
	if (speed > 0.0)
	{
		if (speed > lastSpeed + delAccel) 
			setSpeed = lastSpeed + delAccel;
		else
			setSpeed = speed; 
   	} else if (speed < 0.0)
	{
			if (speed < lastSpeed - delAccel)
				setSpeed = lastSpeed - delAccel;
	    	else
	    		setSpeed = speed;
	} else
    	setSpeed = 0.0;
	lastSpeed = setSpeed;
	return setSpeed;
}

void PandaDrive::SetAccel(float AccelAmount)
{
	if (AccelAmount == 0)
	{
			while (m_leftStick->GetRawButton(m_increaseAccelButton))
			{
				if (m_maxAccel * 1.5 < 5)
					m_maxAccel *= 1.5;
				printf("AccelConst = %0.2f\n", m_maxAccel);
				Wait(.1);
			}
			while (m_leftStick->GetRawButton(m_decreaseAccelButton))
			{
				if (m_maxAccel / 1.5 > .3)
					m_maxAccel /= 1.5;
				printf("AccelConst = %0.2f\n", m_maxAccel);
				Wait(.1);
			}
	}
	else
		m_maxAccel = AccelAmount;
}

float PandaDrive::GetAccelConst()
{
	return m_maxAccel;
}

float PandaDrive::Limit(float num)
{
	if (num > 1.0)
		return 1.0;
	if (num < -1.0)
		return -1.0;
	return num;
}

int PandaDrive::Teleop()
{
	if (m_rightStick->GetRawButton(m_nonSlipButton))
		m_nonSlip = true;
	if (m_rightStick->GetRawButton(m_slipButton))
		m_nonSlip = false;
	if (m_rightStick->GetRawButton(m_tankButton))
		m_tank = true;
	if (m_rightStick->GetRawButton(m_arcadeButton))
		m_tank = false;
	if (m_rightStick->GetRawButton(1)) {
		if (m_tank)
			NonSlipTankDrive();
		else
			NonSlipArcadeDrive();
	}

	SetAccel(0);
	int retval = 0;
	if (m_tank) retval += 1;
	if (m_nonSlip) retval += 2;
	return retval;
}
